#include "StdAfx.h"
#include ".\robot.h"
#include <gl/gl.h>
#include <gl/glu.h>
#include <gl/glut.h>
#include <math.h>

Robot::Robot(void)
{
	SetColor(1.0, 1.0, 0.0);
	m_simulation.SetRobotAngleStep(0.2);
}

Robot::Robot(double lfLength, double lfWidth, double lfXPos, double lfYPos, double lfTheta)
{
	SetColor(1.0, 1.0, 0.0);
	m_simulation.SetRobotParams(lfLength, lfWidth, lfXPos, lfYPos, lfTheta, 0.4);
}

Robot::~Robot(void)
{
}

void Robot::Train(int nTimes, const char * szFileOut, int nEvalFreq, double lfLearningRate, double lfDiscountFactor)
{ 
	m_simulation.Train(nTimes, szFileOut, nEvalFreq, lfLearningRate, lfDiscountFactor); 
	CString strMsg;
	strMsg.Format("Ran %d training iterations, hit the goal %d times, hit obstacles %d times, missec completely %d times",
		nTimes, m_simulation.NumGoalHits(), m_simulation.NumObstacleCollisions(), m_simulation.NumGoalMisses());
	AfxMessageBox(strMsg);
}

void Robot::SetRobotAndGoal() 
{ 
	ClearTrails();
	m_simulation.PlaceRobotAndGoal(); 
}

void Robot::SetAllObstacleParams(int nNumObstacles, int nNumSides, Pos2d p2ObstacleMin, Pos2d p2ObstacleMax, double lfObstacleRad)
{
	return m_simulation.SetAllObstacleParams(nNumObstacles, nNumSides, p2ObstacleMin, p2ObstacleMax, lfObstacleRad);
}

int Robot::PushBotOntoTrail()
{
	if(m_nTrainLength < 400)
	{
		
		double arrLfBackPos[2];
		double arrLfFrontPos[2];
		double arrLfDir[2];
		m_simulation.AccessCollider().GetDrawparams(arrLfBackPos, arrLfFrontPos, arrLfDir);
		
		m_arrPos2dBackPoints[m_nTrainLength][0] = arrLfBackPos[0];
		m_arrPos2dBackPoints[m_nTrainLength][1] = arrLfBackPos[1];

		m_arrPos2dFrontPoints[m_nTrainLength][0] = arrLfFrontPos[0];
		m_arrPos2dFrontPoints[m_nTrainLength][1] = arrLfFrontPos[1];

		m_arrPos2dDir[m_nTrainLength][0] = arrLfDir[0];
		m_arrPos2dDir[m_nTrainLength][1] = arrLfDir[1];

		m_arrLfPrevColors[m_nTrainLength][0] = m_arrLfColor[0];
		m_arrLfPrevColors[m_nTrainLength][1] = m_arrLfColor[1];
		m_arrLfPrevColors[m_nTrainLength][2] = m_arrLfColor[2];

		++m_nTrainLength;
	}
	return m_nTrainLength;
}

void Robot::DrawMe()
{
	double arrLfBackPos[2];
	double arrLfFrontPos[2];
	double arrLfDir[2];

	
	static GLUquadric * pQuadSphere = NULL;
	static GLUquadric * pQuadCyl = NULL;

	if(!pQuadSphere)
	{
		pQuadSphere = gluNewQuadric();
	}

	if(!pQuadCyl)
	{
		pQuadCyl = gluNewQuadric();
	}
	m_simulation.AccessCollider().GetDrawparams(arrLfBackPos, arrLfFrontPos, arrLfDir);
	GLdouble rotMat[16];

	for(int iTrailIter = 0; iTrailIter < m_nTrainLength; ++iTrailIter)
	{

		rotMat[0] = 0.0; rotMat[1] = 0.0; rotMat[2] = -1.0; rotMat[3] = 0.0;
		rotMat[4] = -m_arrPos2dDir[iTrailIter][1]; rotMat[5] = m_arrPos2dDir[iTrailIter][0]; rotMat[6] = 0.0; rotMat[7] = 0.0;
		rotMat[8] = m_arrPos2dDir[iTrailIter][0]; rotMat[9] = m_arrPos2dDir[iTrailIter][1]; rotMat[10] = 0.0; rotMat[11] = 0.0;
		rotMat[12] = m_arrPos2dBackPoints[iTrailIter][0]; rotMat[13] = m_arrPos2dBackPoints[iTrailIter][1]; rotMat[14] = 0.0; rotMat[15] = 1.0;

		glPushMatrix(); // 1

		glLoadIdentity();
		// glTranslated(arrLfBackPos[0], arrLfBackPos[1], 0.0);
		// glRotated(atan2(arrLfDir[1], arrLfDir[0]), 0.0, 0.0, 1.0);
		glMultMatrixd(rotMat);

		glColor3d(m_arrLfPrevColors[iTrailIter][0], 
			m_arrLfPrevColors[iTrailIter][1], 
			m_arrLfPrevColors[iTrailIter][2]);
		gluCylinder(pQuadCyl, 0.5*m_simulation.AccessCollider().GetWidth(), 0.5*m_simulation.AccessCollider().GetWidth(), m_simulation.AccessCollider().GetLength(), 20, 20);
		gluSphere(pQuadSphere, 0.5*m_simulation.AccessCollider().GetWidth(), 20, 20);

		glTranslated(0.0, 0.0, m_simulation.AccessCollider().GetLength());

		gluSphere(pQuadSphere, 0.5*m_simulation.AccessCollider().GetWidth(), 20, 20);

		glPopMatrix();
	}

	rotMat[0] = 0.0; rotMat[1] = 0.0; rotMat[2] = -1.0; rotMat[3] = 0.0;
	rotMat[4] = -arrLfDir[1]; rotMat[5] = arrLfDir[0]; rotMat[6] = 0.0; rotMat[7] = 0.0;
	rotMat[8] = arrLfDir[0]; rotMat[9] = arrLfDir[1]; rotMat[10] = 0.0; rotMat[11] = 0.0;
	rotMat[12] = arrLfBackPos[0]; rotMat[13] = arrLfBackPos[1]; rotMat[14] = 0.0; rotMat[15] = 1.0;

	
	glPushMatrix(); // 1

	glLoadIdentity();
	// glTranslated(arrLfBackPos[0], arrLfBackPos[1], 0.0);
	// glRotated(atan2(arrLfDir[1], arrLfDir[0]), 0.0, 0.0, 1.0);
	glMultMatrixd(rotMat);

	glColor3d(m_arrLfColor[0], m_arrLfColor[1], m_arrLfColor[2]);
	gluCylinder(pQuadCyl, 0.5*m_simulation.AccessCollider().GetWidth(), 0.5*m_simulation.AccessCollider().GetWidth(), m_simulation.AccessCollider().GetLength(), 20, 20);
	gluSphere(pQuadSphere, 0.5*m_simulation.AccessCollider().GetWidth(), 20, 20);

	glTranslated(0.0, 0.0, m_simulation.AccessCollider().GetLength());

	gluSphere(pQuadSphere, 0.5*m_simulation.AccessCollider().GetWidth(), 20, 20);

	glPopMatrix();

	

	glPushMatrix();
	glLoadIdentity();


	glColor3d(0.5, 0.5, 1.0);
	for(int iWhichObstacle = 0 ; iWhichObstacle < m_simulation.GetNumObstacles(); ++iWhichObstacle)
	{
		int nNumPts = m_simulation.AccessObstacle(iWhichObstacle).GetNumPts();
		for(int iPtStart = 0; iPtStart < nNumPts; ++iPtStart)
		{
			int iPtNext = (iPtStart+1)%nNumPts;

			

			arrLfDir[0] = m_simulation.AccessObstacle(iWhichObstacle).GetPolyPts()[iPtNext][0] - 
				m_simulation.AccessObstacle(iWhichObstacle).GetPolyPts()[iPtStart][0];

			arrLfDir[1] = m_simulation.AccessObstacle(iWhichObstacle).GetPolyPts()[iPtNext][1] - 
				m_simulation.AccessObstacle(iWhichObstacle).GetPolyPts()[iPtStart][1];

			double lfDirMag = arrLfDir[0]*arrLfDir[0] + arrLfDir[1]*arrLfDir[1];

			if(lfDirMag < 0.01)
			{
				continue;
			}

			lfDirMag = sqrt(lfDirMag);
			double lfScale = 1.0/lfDirMag;

			arrLfDir[0] *= lfScale;
			arrLfDir[1] *= lfScale;

			glPushMatrix();

			// rotMat[0] = 0.0; rotMat[1] = 0.0; rotMat[2] = -1.0; rotMat[3] = 0.0;
			rotMat[4] = -arrLfDir[1]; rotMat[5] = arrLfDir[0]; // rotMat[6] = 0.0; rotMat[7] = 0.0;
			rotMat[8] = arrLfDir[0]; rotMat[9] = arrLfDir[1]; // rotMat[10] = 0.0; rotMat[11] = 0.0;
			rotMat[12] = m_simulation.AccessObstacle(iWhichObstacle).GetPolyPts()[iPtStart][0]; 
			rotMat[13] = m_simulation.AccessObstacle(iWhichObstacle).GetPolyPts()[iPtStart][1]; 
			rotMat[14] = 5.0; // rotMat[15] = 1.0;

			glMultMatrixd(rotMat);

			gluCylinder(pQuadCyl, 1.0, 1.0, lfDirMag, 10, 10);
			gluSphere(pQuadSphere, 1.0, 10, 10);

			glPopMatrix();
		}
	}

	glColor3d(0.0, 1.0, 0.0);
	glTranslated(m_simulation.GetGoalX(), m_simulation.GetGoalY(), 10.0);

	gluSphere(pQuadSphere, m_simulation.GetGoalRad(), 20, 20);

	glLoadIdentity();

	glColor3d(1.0, 0.0, 0.0);
	glEnable(GL_ALPHA);
	glColor4d(1.0, 0.0, 0.0, 0.5);
	glDisable(GL_LIGHTING);

	for(int iWhichRay = 0; iWhichRay < m_simulation.AccessCollider().RangeFinders().GetNumRays(); ++iWhichRay)
	{
		glBegin(GL_LINES);
		double lfXStart = m_simulation.AccessCollider().RangeFinders().GetPositionArr()[iWhichRay][0];
		double lfYStart = m_simulation.AccessCollider().RangeFinders().GetPositionArr()[iWhichRay][1];

		double lfXDir = m_simulation.AccessCollider().RangeFinders().GetDirectionArr()[iWhichRay][0];
		double lfYDir = m_simulation.AccessCollider().RangeFinders().GetDirectionArr()[iWhichRay][1];

		double lfRange = m_simulation.AccessCollider().RangeFinders().GetRangeArr()[iWhichRay];

		glVertex3d(lfXStart, lfYStart, 2.5);
		glVertex3d(lfXStart+lfRange*lfXDir, 
			lfYStart+lfRange*lfYDir, 2.5);
		glEnd();
	}
	glPopMatrix();
	// ::gluCylinder(&qObj, 
}

bool Robot::Move(int nTurnAmount, int nSpeed)
{
	bool bResult = m_simulation.MoveRobot(nTurnAmount, nSpeed);
	ClearTrails();
	m_simulation.RunRangeFinders();
	return bResult;
}
